
#ifndef SLAM_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_POSE_COST_FUNCTION_2D_H_
#define SLAM_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_POSE_COST_FUNCTION_2D_H_

#include "Eigen/Core"
#include "ceres/ceres.h"
#include <array>

namespace slam {
namespace mapping {
namespace optimization {

// Computes the cost of  'pose' to 'target_pose'.
// Cost increases with the solution's distance from 'target_pose'.
class PoseDeltaCostFunctor2D {
 public:
  static ceres::CostFunction* CreateAutoDiffCostFunction(
      const double translation_weight,
      const double rotation_weight, 
      const std::array<double,3>& target_pose) {
    return new ceres::AutoDiffCostFunction<PoseDeltaCostFunctor2D,
                                           3 /* residuals */,
                                           3 /* pose variables */>(
        new PoseDeltaCostFunctor2D(translation_weight, rotation_weight, target_pose));
  }

  template <typename T>
  bool operator()(const T* const pose, T* residual) const {
    residual[0] = translation_weight_ * (pose[0] - x_);
    residual[1] = translation_weight_ * (pose[1] - y_);
    residual[2] = rotation_weight_ * (
        common::NormalizeAngleDifference(pose[2] - yaw_));
    return true;
  }

 private:
  // Constructs a new TranslationDeltaCostFunctor2D from the given
  // 'target_translation' (x, y).
  explicit PoseDeltaCostFunctor2D(
      const double translation_weight,
       const double rotation_weight,
       const std::array<double,3>& target_pose)
    : translation_weight_(translation_weight),
    rotation_weight_(rotation_weight),
    x_(target_pose[0]),
    y_(target_pose[1]),
    yaw_(target_pose[2]) {}

  PoseDeltaCostFunctor2D(const PoseDeltaCostFunctor2D&) = delete;
  PoseDeltaCostFunctor2D& operator=(
      const PoseDeltaCostFunctor2D&) = delete;

  const double translation_weight_;
  const double rotation_weight_;
  const double x_;
  const double y_;
  const double yaw_;
};

}  // namespace optimization
}  // namespace mapping
}  // namespace slam





#endif